#include <QByteArray>
#include <QXmlAttributes>
#include <QXmlStreamReader>
#include <QString>
#include <QDebug>
#include <QDateTime>

#include "_Interface/_global_datastruct.h"
#include "Backend/wyzkr688rfswbackend.h"
#include "Backend/gtrobotcom.h"
#include "Package/simpletcpserverdll_global.h"
#include "Package/simpletcpsocketdll_global.h"

gtrobotcom::gtrobotcom(){
    this->fromRobotServer = 0;
    this->toRobotSocket = 0;
    this->RobotConnectMark = false;
    this->RobotFirstConnectMark = true;
    this->NewMessageCanRead = false;
    this->NewMessageReadDone = false;
    this->CanSendtoRobot = false;

    for(int i = 0; i < 6; i++){
        this->mRobotDAQData.RobotJointData[i] = 0;
        this->mRobotDAQData.RobotJointTorData[i] = 0;
        this->mRobotDAQData.RobotTTSOffsetData[i] = 0;
    }
    for(int i = 0; i < 8; i++){
        this->mRobotDAQData.RobotVarBoolData[i] = 0;
        this->mRobotDAQData.RoborVarValueData[i] = 0;
    }
    for(int i = 0; i < 3; i++){
        this->mRobotAdjData.toRobotPosTTSData_rela[i] = 0;
        this->mRobotAdjData.toRobotPosTTSData_abs[i] = 0;
    }

    this->mRobotAdjData.toRobotOvn = 100;
}

gtrobotcom::~gtrobotcom(){}

bool  gtrobotcom::getRobotConnectStatus(){
    int nubmer = -1;
    COM_ComServerClientNumber(this->fromRobotServer,nubmer);
    if(nubmer > 0){
        this->RobotConnectMark = true;
    }
    else{
        this->RobotConnectMark = false;
    }
    return this->RobotConnectMark;
}
short gtrobotcom::toRobotTcpServerAdd(){
    COM_ComServeradd(50201,this->fromRobotServer);
    return NOERROR;
}
short gtrobotcom::toRobotTcpServerListen(){
    COM_ComServerlisten(this->fromRobotServer);
    return NOERROR;
}

short gtrobotcom::ReadRobotJointData(double (&data)[6]){
    int i = 0;
    for(i = 0; i < 6; i++){
        data[i] = this->mRobotDAQData.RobotJointData[i];
    }
    return NOERROR;
}
short gtrobotcom::ReadRobotJointTorData(double (&data)[6]){
    int i = 0;
    for(i = 0; i < 8; i++){
        data[i] = this->mRobotDAQData.RobotJointTorData[i];
    }
    return NOERROR;
}

short gtrobotcom::ReadRobotTTSOffsetData(double (&data)[6]){
    int i = 0;
    for(i = 0; i < 6; i++){
        data[i] = this->mRobotDAQData.RobotTTSOffsetData[i];
    }
    return NOERROR;
}

short gtrobotcom::ReadRobotTechVarBoolData(bool (&boolgroup)[8]){
    int i = 0;
    for(i = 0; i < 8; i++){
        boolgroup[i] = this->mRobotDAQData.RobotVarBoolData[i];
    }
    return NOERROR;
}
short gtrobotcom::ReadRobotTechVarValueData(bool (&valueGroup)[8]){
    int i = 0;
    for(i = 0; i < 8; i++){
        valueGroup[i] = this->mRobotDAQData.RoborVarValueData[i];
    }
    return NOERROR;
}

short gtrobotcom::SetRobotPosTTSOffsetData(double data[3]){
    int i = 0;
    for(i = 0; i < 3; i++){
        this->mRobotAdjData.toRobotPosTTSData_rela[i] = data[i];
    }
    return NOERROR;
}


short gtrobotcom::RobotRecvDataDeal(){
    int loopi = 0;
    QXmlStreamReader mQXmlStreamReader;
    mQXmlStreamReader.addData(RecvRawRobotData);
    while(!mQXmlStreamReader.atEnd()){
        if(mQXmlStreamReader.isStartElement()){
            if(mQXmlStreamReader.name() == "AIPos"){
                loopi = 0;
                foreach (const QXmlStreamAttribute & attribute, mQXmlStreamReader.attributes()){
                    QStringRef tempStringRef;
                    QString  tempQString;
                    tempStringRef = attribute.value();
                    tempQString = tempStringRef.toString();
                    this->mRobotDAQData.RobotJointData[loopi] = tempQString.toDouble();
                    loopi++;
                }
            }
            else if(mQXmlStreamReader.name() == "AITor"){
                loopi = 0;
                foreach (const QXmlStreamAttribute & attribute, mQXmlStreamReader.attributes()){
                    QStringRef tempStringRef;
                    QString  tempQString;
                    tempStringRef = attribute.value();
                    tempQString = tempStringRef.toString();
                    this->mRobotDAQData.RobotJointTorData[loopi] = tempQString.toDouble();
                    loopi++;
                }
            }
            else if(mQXmlStreamReader.name() == "Offset"){
                loopi = 0;
                foreach (const QXmlStreamAttribute & attribute, mQXmlStreamReader.attributes()){
                    QStringRef tempStringRef;
                    QString  tempQString;
                    tempStringRef = attribute.value();
                    tempQString = tempStringRef.toString();
                    this->mRobotDAQData.RobotTTSOffsetData[loopi] = tempQString.toDouble();
                    loopi++;
                }
            }
            else if(mQXmlStreamReader.name() == "TecB"){
                loopi = 0;
                foreach (const QXmlStreamAttribute & attribute, mQXmlStreamReader.attributes()){
                    QStringRef tempStringRef;
                    QString  tempQString;
                    tempStringRef = attribute.value();
                    tempQString = tempStringRef.toString();
                    this->mRobotDAQData.RobotVarBoolData[loopi] = tempQString.toInt();
                    loopi++;
                }
            }
            else if(mQXmlStreamReader.name() == "TecV"){
                loopi = 0;
                foreach (const QXmlStreamAttribute & attribute, mQXmlStreamReader.attributes()){
                    QStringRef tempStringRef;
                    QString  tempQString;
                    tempStringRef = attribute.value();
                    tempQString = tempStringRef.toString();
                    this->mRobotDAQData.RoborVarValueData[loopi] = tempQString.toDouble();
                    loopi++;
                }
            }
        }
        mQXmlStreamReader.readNext();
    }
    mQXmlStreamReader.clear();
    this->NewMessageCanRead = true;
    this->NewMessageReadDone = false;
    return NOERROR;
}

bool gtrobotcom::NewMessageBackendReadDone(){
    if(this->NewMessageCanRead == true){
        this->NewMessageCanRead = false;
        this->NewMessageReadDone = true;
        return  true;
    }
    else{
        return false;
    }
    return NOERROR;
}

short gtrobotcom::RobotSendDataDeal(){
    char* tempSend = new char[32];
    QString SendString;
    QXmlStreamWriter stream(&SendString);

    memset(tempSend,'\0',strlen(tempSend));
    stream.writeStartElement("Rob");
        stream.writeAttribute("Type", "Edge");
            stream.writeEmptyElement("Offset");
                sprintf(tempSend,"%.3f", this->mRobotAdjData.toRobotPosTTSData_abs[1]);
                stream.writeAttribute("dy", tempSend);
                sprintf(tempSend,"%.3f", this->mRobotAdjData.toRobotPosTTSData_abs[2]);
                stream.writeAttribute("dz", tempSend);

            stream.writeEmptyElement("OVN");
                sprintf(tempSend,"%.2f", this->mRobotAdjData.toRobotOvn);
                stream.writeAttribute("OVN", tempSend);

    stream.writeEndElement();
    stream.writeEndDocument();

    this->SendRawRobotData = SendString.toLocal8Bit();

    delete []tempSend;
    return NOERROR;
}

short gtrobotcom::toRobotReceiveLoopRun(){
    if(COM_getServerClientReadStatus(fromRobotServer,toRobotSocket)){
        COM_serverReadData(fromRobotServer,toRobotSocket,RecvRawRobotData);
        this->RobotRecvDataDeal();
        this->CanSendtoRobot = true;
    }
    return NOERROR;
}
short gtrobotcom::toRobotSendLoopRun(){
    if(this->getRobotConnectStatus()){
        if(RobotFirstConnectMark == true){
            if(!COM_getServerClientSendStatus(fromRobotServer,toRobotSocket)){
                this->SendRawRobotData = "Connected Done";
                COM_serverSendData(fromRobotServer,toRobotSocket,SendRawRobotData);
                RobotFirstConnectMark = false;
            }
        }
        else{
            if(this->CanSendtoRobot == true){
                if(!COM_getServerClientSendStatus(fromRobotServer,toRobotSocket)){
                    this->RobotSendDataDeal();
                    COM_serverSendData(fromRobotServer,toRobotSocket,SendRawRobotData);
                    this->CanSendtoRobot = false;
                }
            }
        }
    }
    return NOERROR;
}

gtrobotcomfsw::gtrobotcomfsw(){

    this->fromRobotServer = 0;
    this->toRobotSocket = 0;
    this->RobotConnectMark = false;
    this->RobotFirstConnectMark = true;
    this->NewMessageCanRead = false;
    this->NewMessageReadDone = false;
    this->CanSendtoRobot = false;


    for(int i = 0; i < 6; i++){
        this->mRobotDAQData.RobotJointData[i] = 0;
        this->mRobotDAQData.RobotJointTorData[i] = 0;
        this->mRobotDAQData.RobotTTSOffsetData[i] = 0;
    }
    for(int i = 0; i < 8; i++){
        this->mRobotDAQData.RobotVarBoolData[i] = 0;
        this->mRobotDAQData.RoborVarValueData[i] = 0;
    }
    for(int i = 0; i < 3; i++){
        this->mRobotAdjData.toRobotPosTTSData_rela[i] = 0;
        this->mRobotAdjData.toRobotPosTTSData_abs[i] = 0;
    }
}

gtrobotcomfsw::~gtrobotcomfsw(){}

short gtrobotcomfsw::ReadInitPosDone(bool &done){
    this->FswTecData.InitPosDone = mRobotDAQData.RobotVarBoolData[0];
    done = this->mRobotDAQData.RobotVarBoolData[0];
    return NOERROR;
}
short gtrobotcomfsw::ReadEndPosDone(bool &done){
    this->FswTecData.EndPosDone = mRobotDAQData.RobotVarBoolData[1];
    done = this->mRobotDAQData.RobotVarBoolData[1];
    return NOERROR;
}
short gtrobotcomfsw::ReadSpindleSpeed(double& data){
    this->FswTecData.SpindleSpeedReal = mRobotDAQData.RoborVarValueData[0];
    data = this->FswTecData.SpindleSpeedReal;
    return NOERROR;
}
short gtrobotcomfsw::ReadForceControlForceInitSetValue(double &data){
    this->FswTecData.ForceInitPosSet = mRobotDAQData.RoborVarValueData[1];
    data = this->FswTecData.ForceInitPosSet;
    return NOERROR;
}
short gtrobotcomfsw::ReadForceControlForceSetValue(double &data){
    this->FswTecData.ForceProcessSet = mRobotDAQData.RoborVarValueData[2];
    data = this->FswTecData.ForceProcessSet;
    return NOERROR;
}
short gtrobotcomfsw::ReadForceControlStep(double& mstep){
    this->FswTecData.ForceAdjGlobalCoefficient = mRobotDAQData.RoborVarValueData[3];
    mstep = this->FswTecData.ForceAdjGlobalCoefficient;
    return NOERROR;
}
short gtrobotcomfsw::ReadInitPosDelayTime(int &time){
    this->FswTecData.InitPosDelayTime = mRobotDAQData.RoborVarValueData[4];
    time = this->FswTecData.InitPosDelayTime;
    return NOERROR;
}
short gtrobotcomfsw::ReadForceControlForceRealValue(double &data){
    this->FswTecData.ForceSensorReal = mRobotDAQData.RoborVarValueData[5];
    data = this->FswTecData.ForceSensorReal;
    return NOERROR;
}

short gtrobotcomfsw::ReadPointLaserValue(double &data){
    this->FswTecData.PointLaserValue = mRobotDAQData.RoborVarValueData[6];
    data = this->FswTecData.PointLaserValue;
    return NOERROR;
}

short gtrobotcomfsw::WriteRobotFzDirOffset(double Offset){
    mRobotAdjData.toRobotPosTTSData_abs[2] = Offset;
    return NOERROR;
}

short gtrobotcomfsw::WriteRobotOVN(double Ovn){
    mRobotAdjData.toRobotOvn = Ovn;
    return NOERROR;
}





